//加速度加上陀螺仪的连接机械臂的最终代码
#include <ESP8266WiFi.h>
#include <WiFiClient.h>
#include<Servo.h>

const char* ssid = "凌";
const char* password = "92921bc46140";
const char* serverAAddress = "192.168.162.175"; // 服务器A的IP地址
const int serverAPort = 8080;

WiFiClient clientB;

Servo servo5;//左臂
Servo servo6;//右臂
Servo servo7;//钳子
Servo servo8;//底座
//初始化
void setup() {
  Serial.begin(115200);
  connectToWiFi();

  servo5.attach(D5);
  servo6.attach(D6);
  servo7.attach(D7);
  servo8.attach(D8);
}

void loop() {
  if (!clientB.connect(serverAAddress, serverAPort)) {
    Serial.println("Connection to Server A failed");
    delay(5000);
    return;
  }

  Serial.println("Connected to Server A");
  while (clientB.connected()) {
    if (clientB.available()) {
      String linegx = clientB.readStringUntil(',');//接收数据并进行分割
      String linegy = clientB.readStringUntil(',');
      String linegz = clientB.readStringUntil(',');
      String lineax = clientB.readStringUntil(',');
      String lineay = clientB.readStringUntil(',');
      String lineaz = clientB.readStringUntil('\n');

      int angleax = lineax.toInt(); // 将字符串转换为整数
      int angleay = lineay.toInt(); // 将字符串转换为整数
      int angleaz = lineaz.toInt(); // 将字符串转换为整数
      
      float anglegx = linegx.toFloat();//转换为浮点数
      float anglegy = linegy.toFloat();
      float anglegz = linegz.toFloat();

      int servoangleax=map(angleax,-10,10,0,180);//映射角度
      int servoangleay=map(angleay,-10,10,0,180);
      int servoangleaz=map(angleaz,-10,10,0,180);

      int servoanglegx=map(anglegx,-10,10,0,180);
      int servoanglegy=map(anglegy,-10,10,0,180);
      int servoanglegz=map(anglegz,-10,10,0,180);

      servoangleax=constrain(servoangleax,0,180);//调整数值
      servoangleay=constrain(servoangleay,0,180);
      servoangleaz=constrain(servoangleaz,0,180);

      servoanglegx=constrain(servoanglegx,0,180);
      servoanglegy=constrain(servoanglegy,0,180);
      servoanglegz=constrain(servoanglegz,0,180);

      Serial.println(servoanglegx);
      servo5.write(servoangleay);
      servo6.write(servoangleax);
      servo7.write(servoangleay);
      servo8.write(servoanglegz);


    }
  }
  clientB.stop();
  Serial.println("Connection to Server A closed");
  delay(5000);
}

void connectToWiFi() {
  Serial.println();
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());}
//加速度的最终结果